#include "PhysicsNode.h"
#include "PhysicsSystem.h"

#define MIN_VELOCITY 0.0001f

PhysicsNode::PhysicsNode(PhysicsNodeType pnt)	{
	target = NULL;
	resting = false;
	aabb = NULL;
	colVol = NULL;
	m_static = false;
	applyGravity = true;
	parent = NULL;
	type = pnt;
	m_force = Vector3(0,0,0);
	m_invMass = 0;
	dragcoefficient = 0;
	explosionForce = 0;
	freeSpin = false;
	colPenetration = 0;
}

PhysicsNode::PhysicsNode(Quaternion orientation, Vector3 position, PhysicsNodeType pnt) {
	m_orientation	= orientation;
	m_position		= position;
	resting = false;
	aabb = NULL;
	colVol = NULL;
	m_static = false;
	applyGravity = true;
	parent = NULL;
	type = pnt;
	m_force = Vector3(0,0,0);
	m_invMass = 0;
	dragcoefficient = 0;
	explosionForce = 0;
	freeSpin = false;
	colPenetration = 0;
}

PhysicsNode::~PhysicsNode()	{
	delete aabb;
	aabb = NULL;
	delete colVol;
	colVol = NULL;
}

//You will perform your per-object physics integration, here!
//I've added in a bit that will set the transform of the
//graphical representation of this object, too.
void	PhysicsNode::Update(float msec) {
	//FUN GOES HERE
	//f = ma
	//a = f/m

	//Limit force applied
	/*if(m_force.Length() > 50.0f){
		m_force.Normalise();
		m_force = m_force * 50.0f;
	}*/

	bool setResting = false;

	if(!m_static){
		//Not sure if this is worth it?
		//if(abs(m_force.x + m_force.y + m_force.z) > 0){
		if(m_invMass > 0){
			//Drag
			m_force += m_linearVelocity.Inverse()*dragcoefficient;
			m_linearAcceleration = m_force * m_invMass;
		}else{
			m_linearAcceleration = Vector3(0,0,0);
		}
	
		//If no acceleration is being applied and object is moving slowly, put it to rest
		if(abs(m_linearAcceleration.x+m_linearAcceleration.y+m_linearAcceleration.z) == 0 && abs(m_linearVelocity.x+m_linearVelocity.y+m_linearVelocity.z)<MIN_VELOCITY){
			m_linearVelocity = Vector3(0,0,0);
			setResting = true;
			//if(aabb) aabb->isStatic = true;
		}else{
			//Gravity, only apply to moving objects
			if(applyGravity)	m_linearAcceleration.y -= 0.001f;	//constant acceleration due to gravity
			m_position = calcSymplecticEuler(m_position, m_linearVelocity, m_linearAcceleration, msec);
		}

		//Reset force
		m_force = Vector3(0,0,0);


		//Only need to do for not resting...flag in collision?
		//TODO: MODULUS Rotation
		if(!freeSpin){
			m_torque += m_angularVelocity.Inverse()*0.45f;
		}
		m_angularAcceleration = m_invInertia * m_torque;

		//Reset torque
		m_torque = Vector3(0,0,0);

		//If changed to resting, set angular velocity to zero
		if(setResting){
			if(!freeSpin){
				m_angularVelocity = Vector3(0,0,0);
			}
			resting = true;
		}

		if(!resting && !freeSpin && abs(m_angularAcceleration.x+m_angularAcceleration.y+m_angularAcceleration.z) == 0 && abs(m_angularVelocity.x+m_angularVelocity.y+m_angularVelocity.z)<MIN_VELOCITY){
			m_angularVelocity = Vector3(0,0,0);
			resting = true;
		}

		//Normalise all non-resting objects and normalise objects as soon as they rest
		if(!resting || setResting){
			m_angularVelocity += m_angularAcceleration * msec;

			//TODO: optimise this....
			m_orientation = m_orientation + m_orientation*(m_angularVelocity*msec*0.5f);
			m_orientation.Normalise();
		}

		if(target) {
			target->SetTransform(BuildTransform());
		}
	}

}

/*
This function simply turns the orientation and position
of our physics node into a transformation matrix, suitable
for plugging into our Renderer!

It is cleaner to work with matrices when it comes to rendering,
as it is what shaders expect, and allow us to keep all of our
transforms together in a single construct. But when it comes to
physics processing and 'game-side' logic, it is much neater to
have seperate orientations and positions.

*/
Matrix4		PhysicsNode::BuildTransform() {
	Matrix4 m = m_orientation.ToMatrix();

	m.SetPositionVector(m_position);

	return m;
}

Vector3	PhysicsNode::GetForwardDirection(){
	Quaternion q = GetOrientation(); 
	q = q * Vector3(0, 0, -0.5) * q.Conjugate();

	//Vector3 direction = Vector3(q.x, q.y, q.z);

	Vector3 direction = Vector3(q.x, 0, q.z);
	direction.Normalise();
	return direction;
	//myCar->GetPhysicsNode().AddForce2Point(Vector3(q.x * 0.001, q.y * 0.001, q.z * 0.001), Vector3(0, 0, 0));
}


void PhysicsNode::SetCollisionVolume(CollisionVolume*cv)
{
	colVol = cv; 
	colVol->m_pos = &m_position;
}

